

#include "bpc.h"
#include "isp.h"
#include "bget.h"
#include "qe_platform.h"



#define xphash(x,y,w)       ((y) * (w) + (x))



static inline void seq2xp(struct bpc_xpoint *xp, qe_u32 seq)
{
    xp->x      = (seq & 0x00000FFF);
    xp->y      = (seq & 0x00FFF000) >> 12;
    xp->direct = (seq & 0xFF000000) >> 24;
}

static inline qe_u32 xp2seq(struct bpc_xpoint xp)
{
    return (xp.x | (xp.y<<12) | (xp.direct<<24));
}

static inline int xp2bit(qe_dim2 dim, struct bpc_xpoint xp, qe_u8 type)
{
    if (type == BPTAB_BMP_CA) {
       return dim.y * (xp.x + 1) + xp.y;
    } else if (type == BPTAB_BMP_RA) {
        return dim.x * xp.y + xp.x;
    }
    return 0;
}

static inline void bit2xp(struct bpc_xpoint *xp, int bit, qe_dim2 dim, qe_u8 type)
{
    if (type == BPTAB_BMP_CA) {
        xp->x = bit / dim.y;
        xp->y = bit % dim.y;
    } else {
        xp->x = bit % dim.x;
        xp->y = bit / dim.x;
    }
}

static inline void bit_set(int bit, void *p)
{
    qe_u32 *pb = (qe_u32 *)p;
    pb[bit/32] |= 1 << (bit % 32);
}

static qe_err_t bptab_seq_v1_parse(struct bpc_desc *desc, void *tab, unsigned int max_points)
{
    int idx;
    int num_points;
    qe_u32 *data;
    qe_u32 checksum;
    struct bpc_xpoint xp;

    /*
     * The first word indicates how many blind points int the table, and
     * the end word store the checksum
     */

    qe_hexdump_debug(tab, 16);

    data = (qe_u32 *)tab;
    num_points = data[0];
    if (num_points == 0) {
        isp_info("no points in tab");
        return qe_ok;
    } else if (num_points > max_points) {
        isp_error("detect %d points, max:%d", num_points, max_points);
        return qe_err_notsupport;
    }

    /* checksum */
    checksum = qe_checksum_u8(tab, (num_points+1)*sizeof(qe_u32));
    if (checksum != data[max_points-1]) {
        isp_error("checksum error, 0x%x 0x%x", checksum, data[max_points-1]);
        return qe_err_notexist;
    }

    data++;
    for (int i=0; i<num_points; i++) {
        seq2xp(&xp, data[i]);
        xp.flags = BPC_XP_STATIC;
        idx = xphash(xp.x, xp.y, desc->dim.x);
        if (desc->xtab[idx].flags == 0x0) {
            desc->xtab[xphash(xp.x, xp.y, desc->dim.x)] = xp;
            desc->num_static++;
            isp_debug("parse point:(%d,%d)", xp.x, xp.y);
        } else {
            isp_error("conflict point (%d,%d)", xp.x, xp.y);
            return qe_err_exist;
        }
    }

    isp_info("parse %d points", num_points);
    
    desc->num_total = desc->num_static;

    return qe_ok;
}

static qe_err_t bptab_seq_v2_parse(struct bpc_desc *desc, void *tab)
{
    int num_points;
    qe_u32 *data;
    qe_u32 checksum;
    struct bpc_xpoint xp;

    /*
     * The first word indicates how many blind points int the table, and
     * the word after blind points is checksum
     */
    data = (qe_u32 *)tab;
    num_points = data[0];
    if (num_points == 0) {
        isp_info("no points in tab");
        return qe_ok;
    }

    /* checksum */
    checksum = qe_checksum_u8(tab, (num_points+1)*4);
    if (checksum != qe_htonl(data[num_points+1])) {
        isp_error("checksum error");
        return qe_err_notexist;
    }

    data++;
    for (int i=0; i<num_points; i++) {
        seq2xp(&xp, data[i]);
        xp.flags = BPC_XP_STATIC;
        desc->xtab[xphash(xp.x, xp.y, desc->dim.x)] = xp;
        isp_debug("parse (%d,%d)", xp.x, xp.y);
    }

    isp_info("parse %d points", num_points);
    desc->num_total = num_points;
    desc->num_static = num_points;

    return qe_ok;
}

static qe_err_t bptab_bmp_parse(struct bpc_desc *desc, void *tab)
{
    int max_bits;
    int num_points = 0;
    struct bpc_xpoint xp;

    qe_memset(&xp, 0x0, sizeof(struct bpc_xpoint));

    max_bits = desc->dim.x * desc->dim.y;

    for (int i=0; i<max_bits; i++) {
        if (qe_test_bit(i, tab)) {
            bit2xp(&xp, i, desc->dim, desc->fmt);
            xp.flags = BPC_XP_STATIC;
            desc->xtab[xphash(xp.x, xp.y, desc->dim.x)] = xp;
            num_points++;
            isp_debug("parse bit:%d (%d,%d)", i, xp.x, xp.y);
        }
    }

    desc->num_total  = num_points;
    desc->num_static = num_points;

    isp_info("parse %d points", num_points);

    return qe_ok;
}

qe_u32 bpc_xp2seq(struct bpc_xpoint xp)
{
    return xp2seq(xp);
}

void bpc_seq2xp(struct bpc_xpoint *xp, qe_u32 seq)
{
    return seq2xp(xp, seq);
}

int bpc_xp2bit(qe_dim2 dim, struct bpc_xpoint xp, qe_u8 type)
{
    return xp2bit(dim, xp, type);
}

static qe_bool_t gradient_comparator(void *p1, void *p2)
{
    qe_s32 g1, g2;

    g1 = *(qe_s32 *)p1;
    g2 = *(qe_s32 *)p2;

    return (g1 > g2);
}

/**
 * @brief  Calculate the gradient range of the window
 * 
 * @param pdata[in] : start address of the window 
 * @param dim[in]   : frame dim
 * @param min[out]  : pointer of min value
 * @param max[out]  : pointer of max value
 */
static void gradient_range(qe_s32 *pdata, qe_dim2 dim, 
    qe_s32 *min, qe_s32 *max)
{
    int p;
    qe_s32 v;
    qe_s32 garray[8];
    struct qe_minpq *queue;

    garray[0] = 2 * pdata[dim.x * 1 + 1] - pdata[0];
    garray[1] = 2 * pdata[dim.x * 1 + 2] - pdata[2];
    garray[2] = 2 * pdata[dim.x * 1 + 3] - pdata[4];
    garray[3] = 2 * pdata[dim.x * 2 + 3] - pdata[dim.x * 2 + 4];
    garray[4] = 2 * pdata[dim.x * 3 + 3] - pdata[dim.x * 4 + 4];
    garray[5] = 2 * pdata[dim.x * 3 + 2] - pdata[dim.x * 4 + 2];
    garray[6] = 2 * pdata[dim.x * 3 + 1] - pdata[dim.x * 4];
    garray[7] = 2 * pdata[dim.x * 2 + 1] - pdata[dim.x * 2];

    /*
    isp_debug("g[0]:%4d d[1][1]:%3d d[0][0]:%3d", 
        garray[0], pdata[dim.x * 1 + 1], pdata[0]);
    isp_debug("g[1]:%4d d[1][2]:%3d d[0][2]:%3d", 
        garray[1], pdata[dim.x * 1 + 2], pdata[2]);
    isp_debug("g[2]:%4d d[1][3]:%3d d[0][4]:%3d", 
        garray[2], pdata[dim.x * 1 + 3], pdata[4]);
    isp_debug("g[3]:%4d d[2][3]:%3d d[2][4]:%3d", 
        garray[3], pdata[dim.x * 2 + 3], pdata[dim.x * 2 + 4]);
    isp_debug("g[4]:%4d d[3][3]:%3d d[4][4]:%3d", 
        garray[4], pdata[dim.x * 3 + 3], pdata[dim.x * 4 + 4]);
    isp_debug("g[5]:%4d d[3][2]:%3d d[4][2]:%3d", 
        garray[5], pdata[dim.x * 3 + 2], pdata[dim.x * 4 + 2]);
    isp_debug("g[6]:%4d d[3][1]:%3d d[4][0]:%3d", 
        garray[6], pdata[dim.x * 3 + 1], pdata[dim.x * 4]);
    isp_debug("g[7]:%4d d[2][1]:%3d d[2][0]:%3d", 
        garray[7], pdata[dim.x * 2 + 1], pdata[dim.x * 2]);
    */

    /**
     * use MinPQ to sort the gradient array
     */
    queue = qe_minpq_create(sizeof(qe_s32), 8, gradient_comparator);
    if (!queue)
        return;
    
    for (int i=0; i<8; i++) {
        qe_minpq_insert(queue, &garray[i], 0);
    }
    
    for (int i=0; i<8; i++) {
        qe_minpq_delmin(queue, &v, &p);
        if (i == 1) *min = v;
        if (i == 6) {
            *max = v;
            break;
        }
    }

    qe_free(queue);
}

static struct bpc_npoint *new_npoint(qe_u16 x, qe_u16 y, qe_u8 flags)
{
    struct bpc_npoint *p;

    p = (struct bpc_npoint *)bget(sizeof(struct bpc_npoint));
    if (!p)
        return QE_NULL;
    
    p->x     = x;
    p->y     = y;
    p->flags = flags;
    return p;
}

static qe_err_t add_point(struct bpc_desc *desc, struct bpc_npoint *p)
{
    int idx;
    struct bpc_npoint *np;
    struct bpc_xpoint xp;

    idx = xphash(p->x, p->y, desc->dim.x);

    if (desc->xtab[idx].flags == 0x0) {
        xp.x = p->x;
        xp.y = p->y;
        xp.flags = p->flags;
        desc->xtab[idx] = xp;

        if (!(p->flags & BPC_XP_MEM_DYNAMIC)) {
            np = new_npoint(p->x, p->y, p->flags);
        } else {
            np = p;
        }
        qe_list_append(&np->list, &desc->dynamic_list);
        desc->num_dynamic++;
        desc->num_total++;
        isp_debug("point(%d,%d) add hash:%d", xp.x, xp.y, idx);
        return qe_ok;
    }

    isp_error("point(%d,%d) exist", p->x, p->y);
    return qe_err_exist;
}

qe_u32 bpc_frame_detect(struct bpc_desc *desc, void *frame, int kernel_size, 
    qe_s32 thres, qe_list *plist)
{
    int num_points;
    qe_u32 t1, t2;
    qe_s32 *pdata;
    qe_s32 gmin, gmax, gc;
    qe_dim2 win;
    struct bpc_npoint *np;

    gc         = 0;
    gmin       = 0;
    gmax       = 0;
    num_points = 0;
    pdata      = (qe_s32 *)frame;
    win.x      = desc->dim.x - kernel_size + 1;
    win.y      = desc->dim.y - kernel_size + 1;

    t1 = qe_time_ms();
    isp_info("bpc detect start...");

    for (int i=0; i<win.y; i++) {
        
        for (int j=0; j<win.x; j++) {
            
            gradient_range(&pdata[desc->dim.x * i + j], desc->dim, &gmin, &gmax);
            
            gc = pdata[desc->dim.x * (i+2) + (j+2)];

            //isp_debug("g(%d,%d):%d min:%d max:%d", i+2, j+2, gc, gmin, gmax);

            if ((gc > (gmax+thres)) || (gc < (gmin-thres))) {
                np = new_npoint(j+2, i+2, BPC_XP_DETECT);
                if (!np) {
                    isp_error("alloc npoint err");
                    break;
                }
                qe_list_append(&np->list, plist);
                num_points++;
                isp_info("det (%d,%d)", j+2, i+2);
                isp_debug("p(%d,%d) gc:%d gmin:%d gmax:%d", j+2, i+2, 
                    gc, gmin, gmax);
            }
        }
    }

    t2 = qe_time_ms();
    isp_info("bpc detect finish in %dms", t2 - t1);
    isp_info("bpc detect %d pixels", num_points);

    return num_points;
}

void bpc_dynamic_points_remove(struct bpc_desc *desc)
{
    int idx;
    struct bpc_npoint *np, *tmp;

    qe_list_foreach_entry_safe(np, tmp, &desc->dynamic_list, list) {
        isp_debug("rm point(%d,%d)", np->x, np->y);
        idx = xphash(np->x, np->y, desc->dim.x);
        desc->xtab[idx].flags = 0x0;
        qe_list_remove(&np->list);
        brel(np);
        desc->num_dynamic--;
        desc->num_total--;
    }
}

qe_err_t bpc_add_point(struct bpc_desc *desc, struct bpc_npoint *np)
{
    return add_point(desc, np);
}

qe_err_t bpc_add_points(struct bpc_desc *desc, qe_list *plist)
{
    int num_points;
    struct bpc_npoint *np;
    struct qe_list_node *node;

    if (!desc || !desc->xtab || !plist) {
        isp_error("!desc %d !desc->xtab %d !plist %d", 
            !desc, !desc->xtab, !plist);
        return qe_err_param;
    }

    num_points = 0;

    qe_list_foreach(node, plist) {
        np  = qe_list_entry(node, struct bpc_npoint, list);
        if (qe_ok == add_point(desc, np))
            num_points++;
    }

    desc->num_total += num_points;
    isp_info("add %d points to xtab, total %d", num_points, desc->num_total);
    
    return qe_ok;
}

void bpc_desc_init(struct bpc_desc *desc, qe_dim2 dim, qe_u8 fmt, 
    void *comp_addr, unsigned int comp_size)
{
	int xtab_size;

    if (desc->num_total) {
        isp_error("desc inited");
        return;
    }

    xtab_size = dim.x * dim.y * sizeof(struct bpc_xpoint);
    if (comp_size < xtab_size) {
        isp_error("compute size not enough:%d need:%d", comp_size, xtab_size);
        return;
    }

    desc->dim              = dim;
    desc->fmt              = fmt;
    desc->num_total        = 0;
    desc->num_static       = 0;
    desc->num_dynamic      = 0;
    desc->compute_mem_addr = comp_addr;
    desc->compute_mem_size = comp_size;

    qe_list_init(&desc->dynamic_list);

    /* create buffer group */
    bpool(desc->compute_mem_addr, desc->compute_mem_size);

    //desc->xtab = (struct bpc_xpoint *)desc->compute_mem_addr;
    desc->xtab = (struct bpc_xpoint *)bget(xtab_size);
    if (!desc->xtab) {
        isp_error("alloc xtab err");
        return;
    }

    qe_memset(desc->xtab, 0x0, desc->dim.x * desc->dim.y * sizeof(struct bpc_xpoint));
}

qe_err_t bpc_bptab_parse(struct bpc_desc *desc, void *tabaddr)
{
    qe_err_t ret;

    if (!desc || !tabaddr)
        return qe_err_param;

    switch (desc->fmt) {

        case BPTAB_SEQ_4K:
            ret = bptab_seq_v1_parse(desc, tabaddr, 4096);
            break;

        case BPTAB_SEQ_8K:
            ret = bptab_seq_v1_parse(desc, tabaddr, 8192);
            break;

        case BPTAB_SEQ_V2:
            ret = bptab_seq_v2_parse(desc, tabaddr);
            break;

        case BPTAB_BMP_CA:
        case BPTAB_BMP_RA:
            ret = bptab_bmp_parse(desc, tabaddr);
            break;

        default:
            isp_error("unknown fmt:%d", desc->fmt);
            return qe_err_notsupport;
    }

    if (ret != qe_ok) {
        return ret;
    }

    return qe_ok;
}

qe_err_t bpc_bptab_export(struct bpc_desc *desc, qe_u8 fmt, 
    void *expaddr, int expsize)
{
    int idx;
    int bit;
    int max_points;
    int num_export;
    qe_u32 *pseq;

    if (!desc || !expaddr || !expsize)
        return qe_err_param;

    if (!desc->num_total) {
        isp_info("no points");
        return qe_noneed;
    }

    num_export = 0;
    max_points = desc->dim.x * desc->dim.y;
    pseq = (qe_u32 *)expaddr;

    if (fmt == BPTAB_SEQ_4K ||
        fmt == BPTAB_SEQ_8K ||
        fmt == BPTAB_SEQ_V2) {
        pseq[0] = desc->num_total;
    }

    idx = 0;

    for (int i=0; i<max_points; i++) {
        
        if (desc->xtab[i].flags != 0) {
            
            isp_debug("export (%d,%d)", desc->xtab[i].x, desc->xtab[i].y);

            switch (fmt) {

                case BPTAB_SEQ_4K:
                case BPTAB_SEQ_8K:
                case BPTAB_SEQ_V2:
                    pseq[idx+1] = xp2seq(desc->xtab[i]);
                    idx++;
                    break;

                case BPTAB_BMP_CA:
                case BPTAB_BMP_RA:
                    bit = xp2bit(desc->dim, desc->xtab[i], fmt);
                    isp_debug("bit:%d", bit);
                    //qe_set_bit(bit, (unsigned long *)expaddr);
                    bit_set(bit, (void *)expaddr);
                    break;

                default:
                    break;
            }

            if (!(desc->xtab[i].flags | BPC_XP_STATIC)) {
                isp_debug("export (%d,%d)", desc->xtab[i].x, desc->xtab[i].y);
            }
            num_export++;
        }
    }

    if (fmt == BPTAB_SEQ_4K) {
        pseq[4095] = qe_checksum_u8(expaddr, 
            (desc->num_total+1) * sizeof(qe_u32));
    } else if (fmt == BPTAB_SEQ_8K) {
        pseq[8191] = qe_checksum_u8(expaddr, 
            (desc->num_total+1) * sizeof(qe_u32));
    } else if (fmt == BPTAB_SEQ_V2) {
        pseq[desc->num_total] = qe_checksum_u8(expaddr, 
            (desc->num_total+1) * sizeof(qe_u32));
    }

    isp_info("export %d points to 0x%x", desc->num_total, expaddr);
    return qe_ok;
}
